package org.hitlabnz.sensor_fusion_demo.orientationProvider;

import org.hitlabnz.sensor_fusion_demo.representation.Quaternion;

import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorManager;
import android.util.Log;

/**
 *从｛@link Sensor#TYPE_GYROSCOPE提供绝对方向的方向提供程序
 *陀螺仪}和{@link传感器#TYPE_ROTATION_VECTOR Android旋转矢量传感器}。
 *
 *它主要依赖于陀螺仪，但使用Android旋转矢量进行校正，该矢量也提供绝对
 *当前方向的估计。校正是一个静态权重。
 *
 *@作者Alexander Pacha
 *
 */
public class ImprovedOrientationSensor1Provider extends OrientationProvider {

    /**
     * 常数，指定介于纳米秒和秒之间的因子
     */
    private static final float NS2S = 1.0f / 1000000000.0f;

    /**
     *存储陀螺仪获得的差的四元数。
     *基本上，它包含编码为四元数的旋转差。
     *
     *要获得绝对方向，必须通过以下方式将其添加到初始位置
     *将其与另一个四元数相乘
     */
    private final Quaternion deltaQuaternion = new Quaternion();

    /**
     *包含陀螺仪当前旋转（四元数格式的角度和轴）的四元数
     */
    private Quaternion quaternionGyroscope = new Quaternion();

    /**
     *包含旋转矢量传感器获得的绝对方向的四元数。
     */
    private Quaternion quaternionRotationVector = new Quaternion();

    /**
     *用于记录上一次陀螺仪事件发生的时间的时间戳。
     */
    private long timestamp;

    /**
     *这是一个滤波器阈值，用于丢弃低于特定水平的陀螺仪测量值，并且
     *潜在的只是噪声而不是真实的运动。陀螺仪的值通常介于0（停止）和
     *10（快速旋转），因此0.1似乎是过滤噪声的合理阈值（通常小于0.1），并且
     *真实运动（通常>0.1）。请注意，如果使用的是转动
     *设备速度非常慢，因此该值必须在接受噪声（阈值=0）和丢失之间找到平衡
     *用户动作缓慢（阈值>0.5）。0.1似乎适用于大多数应用程序。
     *
     */
    private static final double EPSILON = 0.1f;

    /**
     *给出陀螺仪总速度的值（当设备快速移动时为高，当设备移动时为低
     *设备静止不动）。对于正常运动，这通常是一个介于0和10之间的值。剧烈摇晃的罐子
     *将其增加到约25。请记住，这些值取决于时间，因此更改的采样率
     *传感器将影响此值！
     */
    private double gyroscopeRotationVelocity = 0;

    /**
     *指示方向是否根据旋转矢量初始化的标志。如果为false，则
     *陀螺仪不能使用（因为它只对计算初始状态的差异有意义）。如果
     *是的，
     *陀螺仪可以正常使用。
     */
    private boolean positionInitialised = false;

    /**
     *计数器，用于对旋转矢量和陀螺仪所在的连续帧的数量求和
     *显著不同（并且点积小于0.7）。当
     *旋转矢量的角度爆炸（例如，在快速倾斜期间）或当设备剧烈摇晃时，以及
     *陀螺仪现在完全关闭。
     */
    private int panicCounter;

    /**
     *此重量直接决定旋转传感器将用于校正的量（in
     *传感器融合场景1-传感器选择。陀螺仪和旋转矢量）。必须是介于0和1之间的值。
     *0表示系统完全依赖陀螺仪，而1表示系统完全依靠陀螺仪
     *rotationVector。
     */
    private static final float DIRECT_INTERPOLATION_WEIGHT = 0.005f;

    /**
     *指示旋转向量的异常值的阈值。如果两个矢量之间的点积
     *（陀螺仪定向和旋转矢量定向）下降到该阈值以下（理想情况下它应该是1，
     *如果它们完全相同），则系统仅返回到陀螺仪值，而忽略
     *旋转矢量。
     *
     *当
     *平铺设备。可能的值介于0和1之间，其中接近1的值意味着即使非常小
     *两个传感器之间的差异将被视为异常值，而接近零的值意味着
     *两个传感器之间的几乎任何差异都是可以容忍的。
     */
    private static final float OUTLIER_THRESHOLD = 0.85f;

    /**
     *表示旋转矢量和陀螺仪方向之间存在巨大差异的阈值。
     *如果两个矢量之间的点积
     *（陀螺仪方向和旋转矢量方向）低于此阈值（理想情况下应为1，如果
     *它们完全相同），系统将开始增加恐慌计数器（这可能表明
     *陀螺仪故障）。
     *
     *该值应低于OUTLIER_ THRESHOLD（0.5-0.7）以仅开始增加紧急计数器，
     *当有
     *两个装有保险丝的传感器之间存在巨大差异。
     */
    private static final float OUTLIER_PANIC_THRESHOLD = 0.65f;

    /**
     *阈值，指示已建立混沌状态，而不仅仅是
     *旋转矢量（由快速倾斜过程中的爆炸角度引起）。
     *
     *如果chaosCounter大于此阈值，则当前位置将重置为
     *旋转矢量指示。
     */
    private static final int PANIC_THRESHOLD = 60;

    /**
     *一些用于保存分配的临时变量
     */
    final private float[] temporaryQuaternion = new float[4];
    final private Quaternion correctedQuaternion = new Quaternion();
    final private Quaternion interpolatedQuaternion = new Quaternion();

    /**
     *初始化新的ImprovedOrientationSensor1供应商
     *
     *@param sensorManager 安卓传感器管理器
     */
    public ImprovedOrientationSensor1Provider(SensorManager sensorManager) {
        super(sensorManager);

        //Add the gyroscope and rotation Vector
        sensorList.add(sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE));
        sensorList.add(sensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR));
    }

    @Override
    public void onSensorChanged(SensorEvent event) {

        if (event.sensor.getType() == Sensor.TYPE_ROTATION_VECTOR) {
            //处理旋转矢量（只是安全）
            //计算角度。从API_18开始，Android将提供该值作为event.values[3]，但如果不是，我们必须手动计算。
            SensorManager.getQuaternionFromVector(temporaryQuaternion, event.values);

            // 存储在四元数中
            quaternionRotationVector.setXYZW(temporaryQuaternion[1], temporaryQuaternion[2], temporaryQuaternion[3], -temporaryQuaternion[0]);
            if (!positionInitialised) {
                // Override
                quaternionGyroscope.set(quaternionRotationVector);
                positionInitialised = true;
            }

        } else if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE) {
            //处理陀螺仪并进行融合
            //此时间步长的增量旋转将与当前旋转相乘
            //在根据陀螺仪样本数据计算之后。
            if (timestamp != 0) {
                final float dT = (event.timestamp - timestamp) * NS2S;
                // 旋转采样的轴，尚未归一化。
                float axisX = event.values[0];
                float axisY = event.values[1];
                float axisZ = event.values[2];

                // 计算样品的角速度 Math.sqrt 平方根 这个模是向量在三个坐标轴上分量的平方和的平方根。
                // 这个模代表了向量的长度或大小，在物理学中，它可以表示速度、加速度、力等向量的大小。
                gyroscopeRotationVelocity = Math.sqrt(axisX * axisX + axisY * axisY + axisZ * axisZ);

                // 如果旋转向量足够大以获得轴，则对其进行规格化
                /**
                 * 这行代码是检查陀螺仪测量的旋转速度向量的大小是否足够大，以确定是否存在有效的旋转轴。
                 * 如果旋转速度向量的模（即 gyroscopeRotationVelocity）大于一个很小的阈值 EPSILON，则认为旋转是显著的，并且需要对旋转轴进行标准化。
                 * 这样计算的意义在于将旋转向量转换为单位向量，即方向向量。
                 */
                if (gyroscopeRotationVelocity > EPSILON) {
                    axisX /= gyroscopeRotationVelocity;
                    axisY /= gyroscopeRotationVelocity;
                    axisZ /= gyroscopeRotationVelocity;
                }

                 //围绕该轴与角速度按时间步长积分
                //以便在时间步长上从该样本获得增量旋转
                //我们将转换三角形旋转的轴角表示
                //转换为四元数，然后再将其转换为旋转矩阵。
                double thetaOverTwo = gyroscopeRotationVelocity * dT / 2.0f;
                double sinThetaOverTwo = Math.sin(thetaOverTwo);
                double cosThetaOverTwo = Math.cos(thetaOverTwo);
                deltaQuaternion.setX((float) (sinThetaOverTwo * axisX));
                deltaQuaternion.setY((float) (sinThetaOverTwo * axisY));
                deltaQuaternion.setZ((float) (sinThetaOverTwo * axisZ));
                deltaQuaternion.setW(-(float) cosThetaOverTwo);

                // 移动当前陀螺仪方向
                deltaQuaternion.multiplyByQuat(quaternionGyroscope, quaternionGyroscope);

                //计算点积，以计算两个方向传感器是否已分叉
            //（如果点积更接近0而不是1），因为如果两者相同，它应该接近1。
                float dotProd = quaternionGyroscope.dotProduct(quaternionRotationVector);

                //如果它们已经发散，只依靠陀螺仪（当旋转矢量“跳跃”时，这种情况会发生在某些设备上）。
                if (Math.abs(dotProd) < OUTLIER_THRESHOLD) {
                    // Increase panic counter
                    if (Math.abs(dotProd) < OUTLIER_PANIC_THRESHOLD) {
                        panicCounter++;
                    }

                    //直接使用陀螺仪
                    setOrientationQuaternionAndMatrix(quaternionGyroscope);

                } else {
                    //两者几乎都在说同样的话。进行正常融合。
                //在从陀螺仪和旋转矢量传感器获得的两个绝对四元数之间进行固定权重的插值
                //重量应该很低，所以旋转矢量只能缓慢地校正陀螺仪，并且输出保持响应。
                    quaternionGyroscope.slerp(quaternionRotationVector, interpolatedQuaternion, DIRECT_INTERPOLATION_WEIGHT);

                    //使用陀螺仪和rotationVector之间的插值
                    setOrientationQuaternionAndMatrix(interpolatedQuaternion);
                    //覆盖当前陀螺仪方向
                    quaternionGyroscope.copyVec4(interpolatedQuaternion);

                    //重置紧急计数器，因为两个传感器的读数相同
                    panicCounter = 0;
                }

                if (panicCounter > PANIC_THRESHOLD) {
                    /**
                     * Log.d（“旋转矢量”，
                     * “紧急计数器大于阈值；这表示陀螺仪故障。紧急复位迫在眉睫。”）；
                     */
                    Log.d("Rotation Vector",
                            "Panic counter is bigger than threshold; this indicates a Gyroscope failure. Panic reset is imminent.");

                    if (gyroscopeRotationVelocity < 3) {
                        /**
                         * Log.d（“旋转矢量”，
                         * “正在执行紧急重置。将方向重置为旋转矢量值。”）；
                         */
                        Log.d("Rotation Vector",
                                "Performing Panic-reset. Resetting orientation to rotation-vector value.");

                        // 手动将位置设置为旋转矢量所表示的位置。
                        setOrientationQuaternionAndMatrix(quaternionRotationVector);
                        // 使用校正值覆盖当前陀螺仪方向
                        quaternionGyroscope.copyVec4(quaternionRotationVector);

                        panicCounter = 0;
                    } else {
                        /**
                         * Log.d（“旋转矢量”，
                         * 字符串格式(
                         * “由于正在进行的运动（用户仍在摇晃设备），紧急复位延迟。陀螺仪速度：%.2f>3”，
                         * 陀螺仪旋转速度）；
                         */
                        Log.d("Rotation Vector",
                                String.format(
                                        "Panic reset delayed due to ongoing motion (user is still shaking the device). Gyroscope Velocity: %.2f > 3",
                                        gyroscopeRotationVelocity));
                    }
                }
            }
            timestamp = event.timestamp;
        }
    }

    /**
     *使用提供的四元数设置输出四元数和矩阵，并同步设置
     *
     *@param四元数要设置的四元数（传感器融合的结果）
     */
    private void setOrientationQuaternionAndMatrix(Quaternion quaternion) {
        correctedQuaternion.set(quaternion);
            //我们在deltaQuaternion中反转了w，因为currentOrientationQuaternion需要它。
            //在将其转换回矩阵表示之前，我们需要恢复此过程
        correctedQuaternion.w(-correctedQuaternion.w());

        synchronized (synchronizationToken) {
            //仅使用陀螺仪
            currentOrientationQuaternion.copyVec4(quaternion);
            /**
             * 这段代码是Android系统中处理传感器数据的典型例子，它使用了`SensorManager.getRotationMatrixFromVector()`方法来从四元数中获取旋转矩阵。`SensorManager`是Android API的一部分，提供了访问和处理传感器数据的工具。
             * 具体来说，这段代码的含义如下：
             * 1. **使用陀螺仪数据**：
             *    - `currentOrientationQuaternion.copyVec4(quaternion);` 这一行代码将当前的陀螺仪数据（以四元数的形式表示）复制到`currentOrientationQuaternion`中。这意味着`currentOrientationQuaternion`现在包含了通过陀螺仪测量的旋转信息。
             * 2. **获取旋转矩阵**：
             *    - `SensorManager.getRotationMatrixFromVector(currentOrientationRotationMatrix.matrix, correctedQuaternion.array());` 这一行代码调用了`SensorManager`的`getRotationMatrixFromVector()`方法，它接受两个参数：一个`RotationMatrix`对象和一个四元数数组。
             *    - `currentOrientationRotationMatrix.matrix` 是用于存储旋转矩阵的`RotationMatrix`对象的矩阵数组。
             *    - `correctedQuaternion.array()` 是从陀螺仪数据中计算得到的校正后的四元数数组。
             *    `SensorManager.getRotationMatrixFromVector()` 方法的作用是将四元数转换为旋转矩阵。旋转矩阵是一个3x3的矩阵，它表示了从原始坐标系到当前坐标系的旋转。这个矩阵可以用于多种计算，例如将相机或物体的位置从原始坐标系转换到当前坐标系。
             *    在调用这个方法后，`currentOrientationRotationMatrix.matrix` 数组将包含从陀螺仪数据中计算得到的旋转矩阵。这样，代码就可以同时使用四元数和旋转矩阵来表示和处理旋转信息，这为不同的计算提供了灵活性。
             */
            //将旋转矩阵也设置为同时具有两种表示形式
            SensorManager.getRotationMatrixFromVector(currentOrientationRotationMatrix.matrix, correctedQuaternion.array());
        }
    }
}
